package com.rgbgame.game;

import processing.core.PApplet;

public class SolidStrut extends Strut {
    Vector3d collisionPoint;
    static Vector3d tempA = new Vector3d(0, 0, 0);
    static Vector3d tempB = new Vector3d(0, 0, 0);
    static Vector3d tempC = new Vector3d(0, 0, 0);
    static Vector3d tempD = new Vector3d(0, 0, 0);
    private float friction;

    public SolidStrut(rgbGame rgbGame, Particle A, Particle B) {
        super(rgbGame, A, B);
        collisionPoint = new Vector3d(0f, 0f, 0f);
        friction = 0.5f;
    }

    /*
      * Collide strut with a particle and resolve collision
      */
    public void collideTest(Particle particle) {
        //if (true) return;
        if (!this.colliding || !this.apply) return;
        if (particle == pA || particle == pB) return;
        //float origMass = particle.mass;
        if (particle.pinned) {
            particle.position.set(particle.truePositionLast);
            particle.positionLast.set(particle.truePositionLast);
            collideWithImmobileParticle(particle);
            return;
        }
        //if (particle.pinned) System.out.println("Pinned!");
        //float sumposin = pA.mass*pA.positionLast.x + pB.mass*pB.positionLast.x + particle.mass*particle.positionLast.x;
        //this.applyConstraint();
        Vector3d vold = tempA;
        Vector3d v = particle.position;
        Vector3d.rotateRelative(particle.truePositionLast, pA.truePositionLast, pB.truePositionLast, pA.position, pB.position, vold);
        //Now vold contains the relative old position of p, so we can check for collision
        //this.game.debugCounter++;

        /* This early exit is wrong, fix it!
          float factor = 2.0f * particle.radius;
          float maxX = this.game.max(particle.position.x, vold.x);
          float maxY = this.game.max(particle.position.y, vold.y);
          float minX = this.game.min(particle.position.x, vold.x);
          float minY = this.game.min(particle.position.y, vold.y);
          float leftX = this.game.min(pA.position.x, pB.position.x);
          float rightX = this.game.max(pA.position.x, pB.position.x);
          float topY = this.game.min(pA.position.y, pB.position.y);
          float bottomY = this.game.max(pA.position.y, pB.position.y);
          if ((maxX < leftX - factor) || (minX > rightX + factor)) {
              return;
          }
          if ((maxY < topY - factor) || (minY > bottomY + factor)) {
              return;
          }
          if ((rightX - leftX) < 0.01f && (bottomY - topY) < 0.01f) {
              return;
          }//*/

        Vector3d myDisp = tempB;
        vold.displacementToLineSet(pA.position, pB.position, myDisp);
        myDisp.normalize();
        myDisp.multiply(particle.radius);
        tempC.set(particle.position.x + myDisp.x, particle.position.y + myDisp.y, 0f);
        if (crossing(vold, tempC, pA.position, pB.position, collisionPoint)) {

            //if (particle.pinned) System.out.println("pin");
            boolean usingX = PApplet.abs(pB.position.x - pA.position.x) > PApplet.abs(pB.position.y - pA.position.y);
            v.add(myDisp);
            v.displacementToLineSet(pA.position, pB.position, tempC);
            v.subtract(myDisp);
            tempC.multiply(-1f);
            float propDist, propA, propB; //propDist is 0 if collisionPoint == pA, 1 if pB
            if (usingX) {
                propDist = (collisionPoint.x - pA.position.x) / (pB.position.x - pA.position.x);
            } else {
                propDist = (collisionPoint.y - pA.position.y) / (pB.position.y - pA.position.y);
            }
            propB = pA.mass * propDist / (pA.mass + pB.mass);
            propA = pB.mass * (1f - propDist) / (pA.mass + pB.mass);

            // dcollisionPoint = deltapA*(1-propDist) + deltapB*(propDist);
            //				   = lambda*propA*(1-propDist) + lambda*propB*(propDist)
            //  + lambda(pA.mass*propA+pB.mass*propB)/particle.mass
            double buf = 1.0 / (propA * (1.0 - propDist) + propB * propDist + (pA.mass * propA + pB.mass * propB) / particle.mass);
            double lambdax = buf * tempC.x;
            double lambday = buf * tempC.y;

            /*
                   double lambdanorm = Math.sqrt(lambdax*lambdax+lambday*lambday)+.0001;
                   double normalx = lambdax / lambdanorm;
                   double normaly = lambday / lambdanorm;
                   double parx = normaly;
                   double pary = -normalx;

                   //get incoming "velocities" to do collision response
                   float vainx = pA.position.x-pA.positionLast.x;
                   float vainy = pA.position.y-pA.positionLast.y;
                   float vbinx = pB.position.x-pB.positionLast.x;
                   float vbiny = pB.position.y-pB.positionLast.y;
                   float vpinx = particle.position.x-particle.positionLast.x;
                   float vpiny = particle.position.y-particle.positionLast.y;
                   */

            //project current positions
            pA.position.x += (float) lambdax * propA;
            pA.position.y += (float) lambday * propA;
            pB.position.x += (float) lambdax * propB;
            pB.position.y += (float) lambday * propB;
            particle.position.x -= (float) (lambdax * (pA.mass * propA + pB.mass * propB) / particle.mass);
            particle.position.y -= (float) (lambday * (pA.mass * propA + pB.mass * propB) / particle.mass);


            float abx = (pB.position.x - pA.position.x) / this.distance; //tangent vec
            float aby = (pB.position.y - pA.position.y) / this.distance;
            float vAt = (pA.position.x - pA.positionLast.x) * abx + //projections
                    (pA.position.y - pA.positionLast.y) * aby;
            float vBt = (pB.position.x - pB.positionLast.x) * abx +
                    (pB.position.y - pB.positionLast.y) * aby;
            float vABt = (vAt * pA.mass + vBt * pB.mass) / (pA.mass + pB.mass);
            float vpt = (particle.position.x - particle.positionLast.x) * abx +
                    (particle.position.y - particle.positionLast.y) * aby;
            float c = friction - 1.0f;
            float deltaabout = ((c + 1f) * particle.mass * vpt + vABt * (pA.mass + pB.mass - c * particle.mass)) / (pA.mass + pB.mass + particle.mass) - vABt;
            float deltapout = ((c + 1f) * (pA.mass + pB.mass) * vABt + vpt * (particle.mass - c * (pA.mass + pB.mass))) / (pA.mass + pB.mass + particle.mass) - vpt;

            pA.positionLast.x -= deltaabout * abx;
            pA.positionLast.y -= deltaabout * aby;
            pB.positionLast.x -= deltaabout * abx;
            pB.positionLast.y -= deltaabout * aby;
            particle.positionLast.x -= deltapout * abx;
            particle.positionLast.y -= deltapout * aby;

            //System.out.println("total change: "+
            //		( lambdax*propA*pA.mass + lambdax*propB*pB.mass - lambdax*(pA.mass*propA+pB.mass*propB))
            //				);
            //float sumposina = pA.mass*pA.positionLast.x + pB.mass*pB.positionLast.x + particle.mass*particle.positionLast.x;
            /*
                   float ox = particle.positionLast.x;
                   float oy = particle.positionLast.y;
                   com.rgbgame.game.Vector3d.rotateRelative(particle.position, pA.position, pB.position, pA.positionLast, pB.positionLast, tempC);
                   particle.positionLast.x = friction*tempC.x + (1.0f-friction)*particle.positionLast.x;
                   particle.positionLast.y = friction*tempC.y + (1.0f-friction)*particle.positionLast.y;

                   double dx = (particle.positionLast.x-ox)*(1.0*particle.mass/(pA.mass+pB.mass));
                   double dy = (particle.positionLast.y-oy)*(1.0*particle.mass/(pA.mass+pB.mass));
                   pA.positionLast.x -= (float)dx;//*(pA.mass);// / pA.mass;
                   pA.positionLast.y -= (float)dy;//*(pA.mass);// / pA.mass;
                   pB.positionLast.x -= (float)dx;//*(pB.mass);// / pB.mass;
                   pB.positionLast.y -= (float)dy;//*(pB.mass);// / pB.mass;
                   */

            //float sumposouta = pA.mass*pA.positionLast.x + pB.mass*pB.positionLast.x + particle.mass*particle.positionLast.x;
            /*if (sumposina-sumposouta>.05) {
                       System.out.println("bof change: "+(sumposina-sumposouta));
                       System.out.println("particle: "+ (particle.positionLast.x-ox)*particle.mass);
                       System.out.println("pA: "+ (-dx*pA.mass));
                       System.out.println("pB: "+ (-dx*pB.mass));
                   }*/


        } else {
            return;
        }
        //float sumposout = pA.mass*pA.positionLast.x + pB.mass*pB.positionLast.x + particle.mass*particle.positionLast.x;
        //if (sumposin-sumposout>.05) System.out.println("eof change: "+(sumposin-sumposout));
        //track down where positionLasts are changing!!! something doesn't add up right

    }

    public void collideWithImmobileParticle(Particle particle) {

        if (!this.colliding || !this.apply) return;
        if (particle == pA || particle == pB) return;
        Vector3d vold = tempA;
        Vector3d v = particle.position;
        Vector3d.rotateRelative(particle.truePositionLast, pA.truePositionLast, pB.truePositionLast, pA.position, pB.position, vold);

        //TODO: add early exit

        Vector3d myDisp = tempB;
        vold.displacementToLineSet(pA.position, pB.position, myDisp);
        myDisp.normalize();
        myDisp.multiply(particle.radius);
        tempC.set(particle.position.x + myDisp.x, particle.position.y + myDisp.y, 0f);
        if (crossing(vold, tempC, pA.position, pB.position, collisionPoint)) {
            //if (particle.pinned) System.out.println("pin");
            boolean usingX = PApplet.abs(pB.position.x - pA.position.x) > PApplet.abs(pB.position.y - pA.position.y);
            v.add(myDisp);
            v.displacementToLineSet(pA.position, pB.position, tempC);
            v.subtract(myDisp);
            tempC.multiply(-1f);
            float propDist, propA, propB; //propDist is 0 if collisionPoint == pA, 1 if pB
            if (usingX) {
                propDist = (collisionPoint.x - pA.position.x) / (pB.position.x - pA.position.x);
            } else {
                propDist = (collisionPoint.y - pA.position.y) / (pB.position.y - pA.position.y);
            }
            propB = pA.mass * propDist / (pA.mass + pB.mass);
            propA = pB.mass * (1f - propDist) / (pA.mass + pB.mass);

            // dcollisionPoint = deltapA*(1-propDist) + deltapB*(propDist);
            //				   = lambda*propA*(1-propDist) + lambda*propB*(propDist)
            //  + lambda(pA.mass*propA+pB.mass*propB)/particle.mass (this piece is zero here)
            double buf = 1.0 / (propA * (1.0 - propDist) + propB * propDist);
            double lambdax = buf * tempC.x;
            double lambday = buf * tempC.y;

            //project current positions
            pA.position.x += (float) lambdax * propA;
            pA.position.y += (float) lambday * propA;
            pB.position.x += (float) lambdax * propB;
            pB.position.y += (float) lambday * propB;
            particle.position.x -= (float) (lambdax * (pA.mass * propA + pB.mass * propB) / particle.mass);
            particle.position.y -= (float) (lambday * (pA.mass * propA + pB.mass * propB) / particle.mass);

            //TODO: add friction

        } else {
            return;
        }
        //float sumposout = pA.mass*pA.positionLast.x + pB.mass*pB.positionLast.x + particle.mass*particle.positionLast.x;
        //if (sumposin-sumposout>.05) System.out.println("eof change: "+(sumposin-sumposout));
        //track down where positionLasts are changing!!! something doesn't add up right

    }

    /*
      * Collide strut with an immovable point (and the wall it is connected to) and resolve collision.
      */
    public void collideTest(Vector3d v, SolidWall2d w) {
        /*
           * Better algorithm description:
           * 1) Store vold vector representing last position of v
           *    relative to strut
           * 2) Check for early exit
           * 3) Check if vold->v crosses A.position->B.position
           * 4) If so, solve for TOC; else, exit
           * 5) Find collision point at TOC
           * 6) Resolve collision at TOC by impulse method
           * 7) Finish up time step, return
           *
           * Easier algorithm description: (used here)
           * 1) Same as above through 4)
           * 4) If they cross, find penetration distance
           * 5) Use Jakobsen projection to resolve
           *
           * For each of these, if there is no collision detected, we may wish to
           * also check for against the wall connected to v so as to avoid possible
           * slipthrough.  Fix this next! [done]
           */

        //TODO: add early exit test
        if (!this.colliding || !this.apply) return;
        Vector3d vold = tempA;
        Vector3d.rotateRelative(v, pA.truePositionLast, pB.truePositionLast, pA.position, pB.position, vold);

        //Early exit code here
        Vector3d collisionPoint = tempB;

        if (crossing(vold, v, pA.position, pB.position, collisionPoint) ||
                crossing(w.wallA, w.wallB, pA.position, pB.position, collisionPoint)
                ) {
            //System.out.println("Collision"+w.idNum);
            boolean usingX = PApplet.abs(pB.position.x - pA.position.x) > PApplet.abs(pB.position.y - pA.position.y);

            //dispx and dispy aren't quite correct, because they contain
            //pieces tangential to the strut, so impart some horizontal velocity.
            //[Fixed by using displacementToLineSet instead of collisionPoint]
            //float dispx = v.x-collisionPoint.x;
            //float dispy = v.y-collisionPoint.y;
            Vector3d otherv;
            if (v == w.wallA) otherv = w.wallB;
            else otherv = w.wallA;
            //Make sure we're actually checking closer point on the wall, otherwise we should
            //collide versus the other one (imperfect, but works pretty well in practice)
            if (v.distanceSqr(collisionPoint) > otherv.distanceSqr(collisionPoint)) return;
            v.displacementToLineSet(pA.position, pB.position, tempC);
            tempC.multiply(-1f);
            float propDist, propA, propB; //propDist is 0 if collisionPoint == pA, 1 if pB
            if (usingX) {
                propDist = (collisionPoint.x - pA.position.x) / (pB.position.x - pA.position.x);
            } else {
                propDist = (collisionPoint.y - pA.position.y) / (pB.position.y - pA.position.y);
            }
            propB = pA.mass * propDist / (pA.mass + pB.mass);
            propA = pB.mass * (1f - propDist) / (pA.mass + pB.mass);
            //1.0f - propB;

            //Now we need to move pA and pB by amounts dependent on propA/B so that
            //the collisionPoint gets moved to v, i.e. by dispx/y.
            //The distance that collisionPoint moves is given by:
            // dcollisionPoint = deltapA*(1-propDist) + deltapB*(propDist);
            //				   = lambda*propA*(1-propDist) + lambda*propB*(propDist)
            //We need this to equal disp, so:
            double buf = 1.0f / (propA * (1f - propDist) + propB * propDist);
            //System.out.println(propA+" "+propB);
            double lambdax = buf * tempC.x;//dispx;
            double lambday = buf * tempC.y;//dispy;
            pA.position.x += (float) lambdax * propA;
            pA.position.y += (float) lambday * propA;
            pB.position.x += (float) lambdax * propB;
            pB.position.y += (float) lambday * propB;
            //float newPos = pA.position.y*(1f-propDist)+pB.position.y*(propDist);
            //System.out.println(newPos);
        } else return;
    }

    private boolean crossing(Vector3d v1, Vector3d v2, Vector3d v3, Vector3d v4, Vector3d collisionPoint) {
        double x1 = v1.x;
        double y1 = v1.y;
        double x2 = v2.x;
        double y2 = v2.y;
        double x3 = v3.x;
        double y3 = v3.y;
        double x4 = v4.x;
        double y4 = v4.y;

        double ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3));
        double ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3));
        double denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
        if (Math.abs(denom) < .0000001) {
            //System.out.println("Oh crap");
            return false;
        }
        ua /= denom;
        ub /= denom;
        if ((0 < ua) && (ua < 1) && (0 < ub) && (ub < 1)) {
            collisionPoint.x = (float) (x1 + ua * (x2 - x1));
            collisionPoint.y = (float) (y1 + ua * (y2 - y1));
            collisionPoint.z = 0f;
            return true;
        }
        return false;
    }

}
